import queue
import logging
import threading
import time
import cv2
import numpy as np
from datetime import datetime
from algorithm.line_patrol import judge_task_signal

class Pattern:
    def __init__(self):
        self.run = False
        self.__log_init()


    def __log_init(self):
        self.logger = logging.getLogger("Pattern")


    def open(self, 
             binary_threshold,
             signal_detect_threshold, 
             signal_detect_box,

             door_binary_threshold,
             door_signal_detect_threshold, 
             door_signal_detect_box,
             ):
        self.logger.trace('open.enter')
        
        self.binary_threshold = binary_threshold
        self.signal_detect_threshold = signal_detect_threshold
        self.signal_detect_box = signal_detect_box

        self.door_binary_threshold = door_binary_threshold
        self.door_signal_detect_threshold = door_signal_detect_threshold
        self.door_signal_detect_box = door_signal_detect_box
        
        self.logger.trace('open.done')
        return True


    def close(self):
        self.logger.trace('close.enter')

        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')
        # TODO:
        # 传递的队列需要变成[signal, signal_ratio]的形式
        # signal = "cross" or "nothing" or "door"
        while self.run:
            try:
                frame, number = self.inqueue1.get(timeout=0.1)
                # self.logger.spam(f"get frame {number} from input queue")
            except queue.Empty:
                self.logger.warning("input queue is empty, waiting for next frame...")
                continue
            except Exception as e:
                self.logger.error(f"Error getting frame from input queue: {e}")
                continue

            # 模板匹配：十字路口
            # 转换为HSV颜色空间
            frame_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            hsv = cv2.cvtColor(frame_RGB, cv2.COLOR_BGR2HSV)
            
            # 创建红色掩码（两部分）
            lower_red1 = np.array([0, 100, 100])
            upper_red1 = np.array([15, 255, 255])
            lower_red2 = np.array([165, 100, 100])
            upper_red2 = np.array([179, 255, 255])
            
            mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
            mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
            
            # 合并两个掩码
            mask = cv2.bitwise_or(mask1, mask2)
            
            # 对原始图像和掩码进行位运算
            result = cv2.bitwise_and(frame, frame, mask=mask)
            binary_frame = np.zeros((result.shape[0], result.shape[1]), dtype=np.uint8)
            non_black_pixels = np.any(result > 0, axis=-1)
            binary_frame[non_black_pixels] = 255

            # gray_frame = cv2.cvtColor(result, cv2.COLOR_BGR2GRAY)
            # _, binary_frame = cv2.threshold(gray_frame, self.binary_threshold, 255, cv2.THRESH_BINARY)
            crossing_signal, cross_ratio = judge_task_signal(binary_frame, self.signal_detect_threshold, self.signal_detect_box)
            # self.logger.spam(f"crossing_signal: {crossing_signal}, cross_ratio: {cross_ratio}")
            

            # 模板匹配：门框
            gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            _, binary_frame = cv2.threshold(gray_frame, self.door_binary_threshold, 255, cv2.THRESH_BINARY)
            door_signal, door_ratio = judge_task_signal(binary_frame, self.door_signal_detect_threshold, self.door_signal_detect_box)
            # door_signal = False if door_signal else True
            # self.logger.spam(f"door_signal: {door_signal}, door_ratio: {door_ratio}")


            # 将检测到的路口标志传递给planner
            try:
                if self.outqueue1.full():
                    self.outqueue1.get_nowait()
                self.outqueue1.put([[crossing_signal, door_signal], [cross_ratio, door_ratio]], timeout=0.1)
                # self.logger.spam("signal put")
            except Exception as e:
                self.logger.error('output crossing signal exception: %s', e)
            
            # 将结合模板信息的图像传递给recorder1
            try:
                cv2.putText(
                    frame,
                    f"Signal: {crossing_signal}, {door_signal}, Ratio: {cross_ratio:.2f}, {door_ratio:.2f}",
                    (10, 30),
                    cv2.FONT_HERSHEY_SIMPLEX,
                    1,
                    (255, 0, 0),
                    2
                )
                if self.outqueue2.full():
                    self.outqueue2.get_nowait()
                self.outqueue2.put([frame, number], timeout=0.1)
                # self.logger.spam("image with crossing signal put")
            except Exception as e:
                self.logger.error('output image with crossing signal exception: %s', e)

        self.logger.trace('thread.done')


    def start(self):
        self.logger.trace('start.enter')

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True


    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.handle.join()
        self.logger.trace('stop.done')


    def input_queue_set(self, input_queue_list):
        # 设置输入队列，接受的是一个队列列表
        self.logger.trace('input_queue_set.enter')

        if self.run:
            self.logger.error("pattern is opened, please close the this first!")
            return False
        
        if not isinstance(input_queue_list, list):
            input_queue_list = [input_queue_list]

        for input_queue in input_queue_list:
            if not isinstance(input_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.inqueue1 = input_queue_list[0]                     # 接收来自camera的图像

        self.logger.trace('input_queue_set.done')
        return True


    def output_queue_set(self, output_queue_list):
        # 设置输出队列，接受的是一个队列列表
        self.logger.trace('output_queue_set.enter')

        if self.run:
            self.logger.error("pattern is opened, please close the this first!")
            return False
        
        if not isinstance(output_queue_list, list):
            output_queue_list = [output_queue_list]

        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.outqueue1 = output_queue_list[0]                   # 将是否检测到匹配模板发给planner
        self.outqueue2 = output_queue_list[1]                   # 将结合模板信息的图像发给recorder1

        self.logger.trace('output_queue_set.done')
        return True